//
// Created by jg on 2021/1/7.
//

#ifndef VIS_RVIZ_VIS_RVIZ_H
#define VIS_RVIZ_VIS_RVIZ_H

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <env_edt/plan_map.h>
#include <bezier/bezier_base.h>
#include <bspline/non_uniform_bspline.h>

using std::vector;
using std::shared_ptr;

enum VIS_MAP_TYPE{
    GLOBALMAP, LOCALMAP
};

enum TRAJECTORY_PLANNING_ID {
    BSPLINE = 0,
    BSPLINE_CTRL_PT = 100,
};

class VIS_RVIZ{
protected:
    vector<Eigen::Vector3d> robot_traj_;

    bool is_proj_cube_;
    double vis_traj_width_;

    visualization_msgs::Marker map_marker_, traj_vis_, target_marker_, pr_marker_;
    visualization_msgs::MarkerArray path_vis_, cube_vis_;

    ros::Publisher vis_map_pub_;
    ros::Publisher vis_rob_pub_;
    ros::Publisher vis_target_pub_;
    ros::Publisher vis_corridor_pub_;
    ros::Publisher vis_path_pub_;
    ros::Publisher vis_traj_pub_;
    ros::Publisher vis_pr_pub_;
    ros::Publisher bspline_pub_;
    ros::Publisher traj_pub_;
    vector<ros::Publisher> pubs_;

    MAP_EDT::Ptr map_edt_;
    Bernstein::Ptr bezier_;
public:
    VIS_RVIZ() : is_proj_cube_(true), vis_traj_width_(-1){}

    ~VIS_RVIZ(){}

    void init(ros::NodeHandle& nh);

    void setEnv(MAP_EDT::Ptr& map_edt, Bernstein::Ptr& bezier);

    void visMap(VIS_MAP_TYPE map_type);

    void visRobotPos(Eigen::Vector3d& pt, double yaw);

    void visTargetPt(const Eigen::Vector3d& pt);

    void deleteLastTarget(){target_marker_.points.pop_back();}

    void resetVisRviz();

    void visPath(const vector<Eigen::Vector3d>& path);

    void visCorridor(const vector<Cube>& corridor);

    void visBezierTraj(const Eigen::MatrixXd& poly_coeff, const Eigen::VectorXd& time);

    void visReferenTraj(const Eigen::Vector3d& pr);

    void visBsplineTraj(NonUniformBspline& bspline, double size, const Eigen::Vector4d& color,
                        bool show_ctrl_pts = false, double size2 = 0.1,
                        const Eigen::Vector4d& color2 = Eigen::Vector4d(1, 1, 0, 1), int id1 = 0,
                        int id2 = 0);

    void displaySphereList(const vector<Eigen::Vector3d>& list, double resolution,
                           const Eigen::Vector4d& color, int id, int pub_id = 0);

    typedef shared_ptr<VIS_RVIZ> Ptr;
protected:
    void setMapMarker(VIS_MAP_TYPE map_type);
};

#endif //VIS_RVIZ_VIS_RVIZ_H
